#ifndef GAMEENGINE_GEOMETRY_PARALLELOGRAM3D_HPP
#define GAMEENGINE_GEOMETRY_PARALLELOGRAM3D_HPP

#ifdef GAMEENGINE_NAMESPACES
namespace GameEngine {
#endif

#ifdef _DEBUG
#include <assert.h>
#endif

//=================================================================================
// Implementation of the Parallelogram3d class

//! Default constructor
/*!
 @note Class T should have a default constructor
 */
template <class T>
Parallelogram3d<T>::Parallelogram3d() {}

//! Constructor from an origin and two vectors
/*!
 @param origin The origin of the parallelogram
 @param v1 The first vector of the parallelogram
 @param v2 The second vector of the parallelogram
 @note Class T should have a copy constructor
 */
template <class T>
Parallelogram3d<T>::Parallelogram3d(const Point3d<T>& origin, const Vector3d<T>& v1, const Vector3d<T>& v2) :
	origin_(origin), v1_(v1), v2_(v2) {}

//! Copy constructor
/*!
 @param rhs The parallelogram to copy
 @note Class T should have a copy constructor
 */
template <class T>
Parallelogram3d<T>::Parallelogram3d(const Parallelogram3d& rhs) :
	origin_(rhs.origin_), v1_(rhs.v1_), v2_(rhs.v2_) {}

//! Destructor
/*!
 @note The destructor of class T should be public
 */
template <class T>
Parallelogram3d<T>::~Parallelogram3d() {}

//! Assignment operator
/*!
 @param rhs The parallelogram to copy
 @note Class T should have an assignment operator
 */
template <class T>
Parallelogram3d<T>& Parallelogram3d<T>::operator=(const Parallelogram3d& rhs) {
	if ( &rhs != this ) {
		origin_ = rhs.origin_;
		v1_ = rhs.v1_;
		v2_ = rhs.v2_;
	}
	return *this;
}

//! Accessor to the origin of the parallelogram (read-write)
/*!
 @return A reference to the origin of the parallelogram
 */
template <class T>
Point3d<T>& Parallelogram3d<T>::origin() {
	return origin_;
}

//! Accessor to the origin of the parallelogram (read only)
/*!
 @return A constant reference to the origin of the parallelogram
 */
template <class T>
const Point3d<T>& Parallelogram3d<T>::origin() const {
	return origin_;
}

//! Accessor to the first vector (read-write)
/*!
 @return A reference to the first vector
 */
template <class T>
Vector3d<T>& Parallelogram3d<T>::v1() {
	return v1_;
}

//! Accessor to the first vector (read only)
/*!
 @return A constant reference to the first vector
 */
template <class T>
const Vector3d<T>& Parallelogram3d<T>::v1() const {
	return v1_;
}

//! Accessor to the second vector (read-write)
/*!
 @return A reference to the second vector
 */
template <class T>
Vector3d<T>& Parallelogram3d<T>::v2() {
	return v2_;
}

//! Accessor to the second vector (read only)
/*!
 @return A constant reference to the second vector
 */
template <class T>
const Vector3d<T>& Parallelogram3d<T>::v2() const {
	return v2_;
}

//! Compute the normal of the parallelogram
/*!
 @return The normal of the parallelogram
 @note This function returns the null vector if the parallelogram is degenerate
 */
template <class T>
Vector3d<T> Parallelogram3d<T>::normal() const {
	return v1_.cross(v2_).normalize();
}

//! Compute the point along the parallelogram at given parametric coordinates
/*!
 @param coords The parametric coordinates along the parallelogram
 @return The point along the parallelogram at given parametric coordinates
 */
template <class T>
Point3d<T> Parallelogram3d<T>::point(const Coords2d<T>& coords) const {
	return Point3d<T>(
				origin_.x() + coords.u()*v1_.x() + coords.v()*v2_.x(),
				origin_.y() + coords.u()*v1_.y() + coords.v()*v2_.y(),
				origin_.z() + coords.u()*v1_.z() + coords.v()*v2_.z()
			);
}

//! Compute the parametric coordinates of given point along the parallelogram
/*!
 *  If given point does not belong to the plane of the parallelogram, the coordinates
 *  of its orthogonal projection along the plane are returned.
 *
 *  Each time this function is called, a transformer has to be created. If you want
 *  to convert multiple points to parametric coordinates, it is much more efficient
 *  to directly access the transformer and use the transform() function.
 *
 @param pt The point whose parametric coordinates should be computed
 @return The parametric coordinates of the given point
 */
template <class T>
Coords2d<T> Parallelogram3d<T>::coordinates(const Point3d<T>& pt) const {
	Transformer3d<T> t = transformer();
	Point3d<T> tmp = transformer.transform(pt);
	return Coords2d<T>(tmp.x(), tmp.y());
}

//! Build a transformer that converts points in the Cartesian space to parametric coordinates
/*!
 @return A transformer that converts points in the Cartesian space to parametric coordinates
 */
template <class T>
Transformer3d<T> Parallelogram3d<T>::transformer() const {
	Vector3d<T> n = normal();
	return Transformer3d<T>(
		origin_, point(Coords2d<T>(1,0)), point(Coords2d<T>(0,1)), Point3d<T>(origin_.x()+n.x(),origin_.y()+n.y(),origin_.z()+n.z()),
		Point3d<T>(0,0,0), Point3d<T>(1,0,0), Point3d<T>(0,1,0), Point3d<T>(0,0,1));
}

//! Compute the location of the given point
/*!
 @return The given point
 */
template <class T>
Point3d<T> Parallelogram3d<T>::operator[](Parallelogram3D::Index index) const {
	if ( index == Parallelogram3D::First ) return origin_;
	if ( index == Parallelogram3D::Second ) return point(Coords2d<T>(1,0));
	if ( index == Parallelogram3D::Third ) return point(Coords2d<T>(0,1));
	return point(Coords2d<T>(1,1));
}

//! Test wether a point is inside the parallelogram plane
/*!
 @param rhs The point to test for coplanarity
 @param tolerance It is unlikely that the point falls exactly on the
                  parallelogram plane. The tolerance parameter allows
				  for approximate coplanarity
 @return True if the point is inside the parallelogram plane, false otherwise
 */
template <class T>
bool Parallelogram3d<T>::coplanar(const Point3d<T>& rhs, const T& tolerance) const {
	// Get the parallelogram normal
	Vector3d<T> n = normal();

	// Compute the dot product with the given point
	T dot = n.dot(Vector3d<T>(points_[0], rhs));

	// If the dot product is close enough to 0, point is considered coplanar with the parallelogram
	return ( dot <= tolerance );
}

//! Test wether a point is inside the parallelogram
/*!
 @param rhs The point to test for inclusion
 @param strict If set to true, a point located exactly on the perimeter of
			   the parallelogram is not considered to belong to the parallelogram
 @return True if the point is inside the parallelogram, false otherwise
 */
template <class T>
bool Parallelogram3d<T>::inside(const Point3d<T>& rhs, bool strict) const {
	// Check whether point is in the parallelogram plane
	if ( coplanar(rhs) == false ) return false;

	// Get the parallelogram normal
	Vector3d<T> n = normal();

	// Get the four parallelogram points
	const Point3d<T>& p1 = origin_;
	Point3d<T> p2 = point(Coords2d<T>(1,0));
	Point3d<T> p3 = point(Coords2d<T>(1,1));
	Point3d<T> p4 = point(Coords2d<T>(0,1));

	// Get the sides of the parallelogram
	T d1 = n.dot( Vector3d<T>(p1, p2).cross(Vector3d<T>(p1, rhs)) );
	T d2 = n.dot( Vector3d<T>(p2, p3).cross(Vector3d<T>(p2, rhs)) );
	T d3 = n.dot( Vector3d<T>(p3, p4).cross(Vector3d<T>(p3, rhs)) );
	T d4 = n.dot( Vector3d<T>(p4, p1).cross(Vector3d<T>(p4, rhs)) );

	// Check the parallelogram belonging
	if ( strict ) {
		// Check whether point lies on one of the (extended) parallelogram edges
		// If it does, point does not strictly belong to the parallelogram
		if ( d1 == 0 ) return false;
		if ( d2 == 0 ) return false;
		if ( d3 == 0 ) return false;
		if ( d4 == 0 ) return false;

		// Check the sides
		if ( ( d1 < 0 ) != ( d2 < 0 ) ) return false;
		if ( ( d1 < 0 ) != ( d3 < 0 ) ) return false;
		if ( ( d1 < 0 ) != ( d4 < 0 ) ) return false;
	}
	else {
		// Get the first non-null dot product (i.e., the first edge along which point does not lie)
		T d = ( d1 == 0 ? ( d2 == 0 ? ( d3 == 0 ? d4 : d3 ) : d2 ) : d1 );

		// Check the sides if and only if point is not on the edge
		if ( ( d1 != 0 ) && ( ( d <= 0 ) != ( d1 <= 0 ) ) ) return false;
		if ( ( d2 != 0 ) && ( ( d <= 0 ) != ( d2 <= 0 ) ) ) return false;
		if ( ( d3 != 0 ) && ( ( d <= 0 ) != ( d3 <= 0 ) ) ) return false;
		if ( ( d4 != 0 ) && ( ( d <= 0 ) != ( d4 <= 0 ) ) ) return false;
	}

	// If all sides are the same, point is in the parallelogram
	return true;
}


//! Compute the intersection between the line and the parallelogram
/*!
 @param rhs The line to intersect with the parallelogram
 @param line_coordinate The parametric line coordinate of the intersection between
                        the parallelogram and the line, if both do intersect. To
					    retrieve the intersection point, call rhs.point(line_coordinate)
 @param coordinates The parallelogram parametric coordinates of the intersection
                    between the parallelogram and the line, if both do intersect. To
					retrieve the intersection point, call point(coordinates)
 @param type If not nil, this parameter receives the type of intersection
             between the parallelogram and the line. The intersection type
			 can be:<BR>
			 NoIntersection if the line does not intersect the parallelogram,<BR>
			 ExternIntersection if intersection occurs out of the parallelogram,<BR>
			 EdgeIntersection if intersection occurs on one of the edges of
			 the parallelogram,<BR>
			 Parallel if the line is parallel to the parallelogram,<BR>
			 Intersection if the line intersects the parallelogram
 @return True if the line intersects the parallelogram, false otherwise
 @note Type T should have an implicit conversion to type double
 */
template <class T>
bool Parallelogram3d<T>::intersection(const Line3d<T>& rhs, T& line_coordinate, Coords2d<T>& coordinates, IntersectionType::Type* type) const {
	// Get the three parallelogram points
	const Point3d<T>& p1 = origin_;
	Point3d<T> p2 = point(Coords2d<T>(1,0));
	Point3d<T> p3 = point(Coords2d<T>(1,1));

	// Compute denominator
	T den = (
		  p3.x()*((p1.y() - p2.y())*(rhs.p1().z() - rhs.p2().z()) - (rhs.p1().y() - rhs.p2().y())*(p1.z() - p2.z()))
		+ p2.x()*(-(p1.y() - p3.y())*(rhs.p1().z() - rhs.p2().z()) + (rhs.p1().y() - rhs.p2().y())*(p1.z() - p3.z()))
		+ p1.x()*((p2.y() - p3.y())*(rhs.p1().z() - rhs.p2().z()) - (rhs.p1().y() - rhs.p2().y())*(p2.z() - p3.z()))
		+ (rhs.p1().x() - rhs.p2().x())*(p3.y()*(p1.z() - p2.z()) + p1.y()*(p2.z() - p3.z()) + p2.y()*(-p1.z() + p3.z()))
	);

	// The line is parallel to the parallelogram
	if ( den == 0 ) {
		if ( type ) *type = IntersectionType::Parallel;
		return false;
	}

	// Compute U coordinate
	coordinates.u() = (
		  p3.x()*(p1.y()*(rhs.p1().z() - rhs.p2().z()) + rhs.p1().y()*(rhs.p2().z() - p1.z()) + rhs.p2().y()*(-rhs.p1().z() + p1.z()))
		+ p1.x()*(p3.y()*(-rhs.p1().z() + rhs.p2().z()) + rhs.p2().y()*(rhs.p1().z() - p3.z()) + rhs.p1().y()*(-rhs.p2().z() + p3.z()))
		+ rhs.p2().x()*(p3.y()*(rhs.p1().z() - p1.z()) + rhs.p1().y()*(p1.z() - p3.z()) + p1.y()*(-rhs.p1().z() + p3.z()))
		+ rhs.p1().x()*(p3.y()*(-rhs.p2().z() + p1.z()) + p1.y()*(rhs.p2().z() - p3.z()) + rhs.p2().y()*(-p1.z() + p3.z()))
	) / den;

	// Compute V coordinate
	coordinates.v() = -(
		  p2.x()*(p1.y()*(rhs.p1().z() - rhs.p2().z()) + rhs.p1().y()*(rhs.p2().z() - p1.z()) + rhs.p2().y()*(-rhs.p1().z() + p1.z()))
		+ p1.x()*(p2.y()*(-rhs.p1().z() + rhs.p2().z()) + rhs.p2().y()*(rhs.p1().z() - p2.z()) + rhs.p1().y()*(-rhs.p2().z() + p2.z()))
		+ rhs.p2().x()*(p2.y()*(rhs.p1().z() - p1.z()) + rhs.p1().y()*(p1.z() - p2.z()) + p1.y()*(-rhs.p1().z() + p2.z()))
		+ rhs.p1().x()*(p2.y()*(-rhs.p2().z() + p1.z()) + p1.y()*(rhs.p2().z() - p2.z()) + rhs.p2().y()*(-p1.z() + p2.z()))
	) / den;

	// Compute line coordinate
	line_coordinate = (
		  p3.x()*(p2.y()*(-rhs.p1().z() + p1.z()) + p1.y()*(rhs.p1().z() - p2.z()) + rhs.p1().y()*(-p1.z() + p2.z()))
		+ p2.x()*(p3.y()*(rhs.p1().z() - p1.z()) + rhs.p1().y()*(p1.z() - p3.z()) + p1.y()*(-rhs.p1().z() + p3.z()))
		+ p1.x()*(p3.y()*(-rhs.p1().z() + p2.z()) + p2.y()*(rhs.p1().z() - p3.z()) + rhs.p1().y()*(-p2.z() + p3.z()))
		+ rhs.p1().x()*(p3.y()*(p1.z() - p2.z()) + p1.y()*(p2.z() - p3.z()) + p2.y()*(-p1.z() + p3.z()))
	) / den;

	// Check if intersection occurs out of the parallelogram
	if ( in_parallelogram(coordinates) == false ) {
		if ( type ) *type = IntersectionType::ExternIntersection;
		return false;
	}

	// Check if intersection occurs on the parallelogram edges
	if ( type ) {
		if ( in_parallelogram(coordinates, true) == false ) {
			*type = IntersectionType::EdgeIntersection;
		}
		else *type = IntersectionType::Intersection;
	}
	return true;
}

//! Compute the parametric coordinates of the closest point along the parallelogram
/*!
 @param rhs The point to project on the parallelogram
 @param coords Receives the parametric coordinates of the closest point along the parallelogram
 @return True if parametric coordinates of the closest point could be computed (this function
         should return false only if the parallelogram is degenerate)
 @note If you want to get back to the point location, call the point(coords) function
 */
template <class T>
bool Parallelogram3d<T>::closest_point(const Point3d<T>& rhs, Coords2d<T>& coords) const {
	// Create a line with the normal
	Vector3d<T> n = normal();
	Line3d<T> line(rhs, Point3d<T>(rhs.x()+n.x(), rhs.y()+n.y(), rhs.z()+n.z()));

	// Compute the intersection between the line and the parallelogram
	IntersectionType::Type type;
	Coords2d<T> coordinates;
	T line_coordinate;
	bool test = intersection(line, line_coordinate, coordinates, &type);
	#ifdef _DEBUG
		assert( test );
	#endif
	if ( test == false ) {
		coords = Coords2d<T>(0,0);
		return false;
	}

	// Check if intersection point is in the parallelogram
	if ( in_parallelogram(coordinates) ) {
		coords = coordinates;
		return true;
	}

	// If it is not in the parallelogram, find the closest parallelogram point
	Point3d<T> i = point(coordinates);

	// Get the four parallelogram points
	Point3d<T> pts[] = {
		origin_,
		point(Coords2d<T>(1,0)),
		point(Coords2d<T>(1,1)),
		point(Coords2d<T>(0,1))
	};

	// Find the two closest points
	int index1 = 0, index2 = 1;
	if ( i.get_square_distance(pts[0]) > i.get_square_distance(pts[1]) ) {
		index1 = 1;
		index2 = 0;
	}
	if ( i.get_square_distance(pts[index2]) > i.get_square_distance(pts[2]) ) {
		if ( i.get_square_distance(pts[index1]) > i.get_square_distance(pts[2]) ) {
			index2 = index1;
			index1 = 2;
		}
		else {
			index2 = 2;
		}
	}
	if ( i.get_square_distance(pts[index2]) > i.get_square_distance(pts[3]) ) {
		index2 = 3;
	}

	// Test if point falls out of the line
	Line3d<T> closest_line(pts[index1], pts[index2]);
	T param = closest_line.closest_point(rhs);
	if ( param <= 0 ) {
		if ( index1 == 0 ) {
			coords = Coords2d<T>(0,0);
			return true;
		}
		if ( index1 == 1 ) {
			coords = Coords2d<T>(1,0);
			return true;
		}
		if ( index1 == 2 ) {
			coords = Coords2d<T>(1,1);
			return true;
		}
		coords = Coords2d<T>(0,1);
		return true;
	}
	if ( param >= 1 ) {
		if ( index2 == 0 ) {
			coords = Coords2d<T>(0,0);
			return true;
		}
		if ( index2 == 1 ) {
			coords = Coords2d<T>(1,0);
			return true;
		}
		if ( index2 == 2 ) {
			coords = Coords2d<T>(1,1);
			return true;
		}
		coords = Coords2d<T>(0,1);
		return true;
	}

	// Test if point falls in the line
	Coords2d<T> res(0,0);
	if ( index1 == 1 ) res.u() += ( (T)1.0 - param );
	else if ( index1 == 2 ) {
		// TODO - check
		res.u() += param;
		res.v() += ( (T)1.0 - param );
	}
	else if ( index1 == 2 ) res.v() += ( (T)1.0 - param );
	if ( index2 == 1 ) res.u() += param;
	else if ( index2 == 2 ) {
		// TODO - check
		res.u() += ( (T)1.0 - param );
		res.v() += param;
	}
	else if ( index2 == 3 ) res.v() += param;

	// Return closest point
	coords = res;
	return true;
}

//! Compute the closest point along the parallelogram
/*!
 @return The closest point along the parallelogram
 @param rhs The point to project on the parallelogram
 @note This is an overloaded function, provided for convenience and for compatibility
       with some template classes. It is slower than the function
	   closest_point(const Point3d<T>& rhs, Coords2d<T>& coords). Unless you explicitly
	   need the point location, its use is therefore not advised
 */
template <class T>
Point3d<T> Parallelogram3d<T>::closest_point(const Point3d<T>& rhs) const {
	// Get the point coordinates
	Coords2d<T> coordinates;
	closest_point(rhs, coordinates);

	// Return the point location
	return point(coordinates);
}

//! Compute the perimeter of the parallelogram
/*!
 @return The perimeter of the parallelogram
 */
template <class T>
T Parallelogram3d<T>::perimeter() const {
	return (T)2*v1_.length() + (T)2*v2_.length();
}

//! Compute the area of the parallelogram
/*!
 @return The area of the parallelogram
 */
template <class T>
T Parallelogram3d<T>::area() const {
	return (T)( Vector3d<T>::cross( Vector3d<T>(points_[0], points_[1]), Vector3d<T>(points_[0], points_[2]) ).length() );
}

//! Check if the parallelogram is degenerate
/*!
 @return True if the parallelogram is degenerate, false otherwise
 @note The parallelogram is considered degenerate if its area is equal
       or below the tolerance threshold
 */
template <class T>
bool Parallelogram3d<T>::degenerate(const T& tolerance) const {
	return ( area() <= tolerance );
}

//! Check if the coordinates are in the parallelogram
/*!
 @param coords The parametric coordinates to test
 @param strict If set to true, points located on the edges of the parallelogram
               are not considered to belong to the parallelogram
 @return True if the coordinates are in the parallelogram, false otherwise
 */
template <class T>
bool Parallelogram3d<T>::in_parallelogram(const Coords2d<T>& coords, bool strict) {
	if ( strict ) return ( ( coords.u() >  0 ) && ( coords.v() >  0 ) && ( coords.u() <  1 ) && ( coords.v() <  1 ) );
	/* else */    return ( ( coords.u() >= 0 ) && ( coords.v() >= 0 ) && ( coords.u() <= 1 ) && ( coords.v() <= 1 ) );
}

//! Compute the plane in which the parallelogram lies
/*!
 @return The plane in which the parallelogram lies
 */
template <class T>
Plane3d<T> Parallelogram3d<T>::plane() const {
	return Plane3d<T>(origin_, normal());
}

#ifdef GAMEENGINE_NAMESPACES
} // namespace GameEngine
#endif

#endif
